/*
 * main.cpp
 *
 *  Created on: 10/11/2011
 *      Author: will
 */

#include <ctime>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cstdlib>
#include <string>
using std::cout;
using std::endl;
using std::cerr;
using std::string;
using std::ifstream;
using std::ofstream;

#include <libconfig.h++>
using namespace libconfig;

#include "catgraph/CatGraph.h"

int main(int argc, char* argv[]) {

	// seed random number gen
	srand ( time(NULL) );

	// load config file

	if (argc < 2) {
		std::cerr << "Config file not specified." << std::endl;
		return(EXIT_FAILURE);
	}

	Config cfg;

	try {
		cfg.readFile(argv[1]);
	} catch (const FileIOException &fioex) {
		std::cerr << "I/O error while reading file." << std::endl;
		return (EXIT_FAILURE);
	} catch (const ParseException &pex) {
		std::cerr << "Parse error while reading file." << std::endl;
		return (EXIT_FAILURE);
	}

	std::string name = cfg.lookup("name");
	std::string dataset(argv[2]);
	//std::string dataset = cfg.lookup("dataset");
	cout << "Using file: " << name << endl;
	cout << "Using dataset: " << dataset << endl << endl;

	const Setting& config = cfg.getRoot();

	ifstream odometryData(config[dataset.c_str()]["odometryPath"]);

	Matrix3d Q;
	Vector3d u_mean;
	Q << pow(config["motionError"]["x"], 2), 0, 0, 0, pow(
			config["motionError"]["y"], 2), 0, 0, 0, pow(
			config["motionError"]["r_z"], 2);
	u_mean << config["averageOdometry"]["x"],
			  config["averageOdometry"]["y"],
			  config["averageOdometry"]["r_z"];

	CatGraph graph(config["numParticles"],config["numNodes"],Q,u_mean);

	double essThreshold = config["essThreshold"];
	double spreadFraction = config["spreadFraction"];
	double reverseFraction = config["reverseFraction"];
	double hypothesisSize = config["hypothesisSize"];
	double hypothesisThreshold = config["hypothesisThreshold"];




}
